#include "common_inc.h"
#include "configurations.h"
#include <string.h>
#include <stdlib.h>
#include "bsp_uart.h"
#include "bsp_can.h"
#include "can.h"

extern Motor motor;
extern EncoderCalibrator encoderCalibrator;
#define debugPrintf(str, ...) 
uint8_t CANTxData[8];

uint8_t check_sum(uint8_t *data, uint16_t len)
{
    uint8_t sum = 0;
    for (int i = 0; i < len; i++)
    {
        sum += data[i];
    }
    return sum;
}

void uart1_send_pack(uint8_t id, uint8_t cmd, uint8_t *data, uint16_t len)
{
    if (len > 8) len = 8;
    // [head], [4bit node ID], [7bit cmd], [8Byte data], [check sum]
    uint8_t pack_buf[12] = {0x3E,                                
                            (uint8_t)(id & 0x0F), 
                            (uint8_t)(cmd & 0x7F),                            
                            0, 0, 0, 0, 0, 0, 0, 0,              
                            0};                                  
    memcpy(&pack_buf[3], data, len);
    pack_buf[11] = check_sum(pack_buf, 11);
    HAL_UART_Transmit(&huart1, pack_buf, sizeof(pack_buf), 0x0F);
}


static void can1_send_pack(uint8_t cmd, uint8_t *data, uint16_t len)
{    
    uint8_t data_buf[8] = {0}; // 8Byte data 
    if (len > 8) len = 8;
    memcpy(&data_buf[0], data, len);
    CAN_Send_Data(boardConfig.canNodeId, cmd, data_buf);
}

void send_interface(void *arg, uint8_t cmd, uint8_t *data, uint16_t len)
{
    if (arg == (void*)&huart1)
    {
        uart1_send_pack(boardConfig.canNodeId, cmd, data, len);
    }
    else if (arg == (void*)&hcan)
    {
        can1_send_pack(cmd, data, len);
    }
    else if (arg == (void*)&huart3)
    {

    }
}

/**
 * @brief 
 * @param arg 
 * @param cmd 0~0x7F
 * @param RxData 8 byte
 */
void interface_base(void *arg, uint8_t cmd, const uint8_t *RxData)
{
    float tmpF;
    int32_t tmpI;
    switch (cmd)
    {
        // 0x00~0x0F No Memory CMDs
        case 0x01:  // Enable Motor
            motor.controller->requestMode = (*(uint32_t*) (RxData) == 1) ?
                                            Motor::MODE_COMMAND_VELOCITY : Motor::MODE_STOP;
            break;
        case 0x02:  // Do Calibration
            encoderCalibrator.isTriggered = true;
            break;
        case 0x03:  // Set Current SetPoint
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_CURRENT)
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_CURRENT);
            motor.controller->SetCurrentSetPoint((int32_t) (*(float*) RxData * 1000));
            break;
        case 0x04:  // Set Velocity SetPoint
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_VELOCITY)
            {
                motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_VELOCITY);
            }
            motor.controller->SetVelocitySetPoint(
                (int32_t) (*(float*) RxData *
                           (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
            break;
        case 0x05:  // Set Position SetPoint
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
            {
                motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_POSITION);
            }
            motor.controller->SetPositionSetPoint(
                (int32_t) (*(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
            debugPrintf("SET MOTOR[0x05] POSITION[]\r\n");
            if (RxData[4]) // Need Position & Finished ACK
            {
                tmpF = motor.controller->GetPosition();
                memcpy(CANTxData, &tmpF, 4);
                CANTxData[4] = motor.controller->state == Motor::STATE_FINISH ? 1 : 0;
            }
            send_interface(arg, 0x23, CANTxData, 5);
            break;
        case 0x06:  // Set Position with Time
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_POSITION);
            motor.controller->SetPositionSetPointWithTime(
                (int32_t) (*(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS),
                *(float*) (RxData + 4));
            if (RxData[4]) // Need Position & Finished ACK
            {
                tmpF = motor.controller->GetPosition();
                memcpy(CANTxData, &tmpF, 4);
                CANTxData[4] = motor.controller->state == Motor::STATE_FINISH ? 1 : 0;
                send_interface(arg, 0x23, CANTxData, 5);
            }
            break;
        case 0x07:  // Set Position with Velocity-Limit
        {
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
            {
                motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_POSITION);
            }
            motor.config.motionParams.ratedVelocity =
                (int32_t) (*(float*) (RxData + 4) * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
            motor.controller->SetPositionSetPoint(
                (int32_t) (*(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
            // Always Need Position & Finished ACK
            tmpF = motor.controller->GetPosition();
            memcpy(CANTxData, &tmpF, 4);
            CANTxData[4] = motor.controller->state == Motor::STATE_FINISH ? 1 : 0;
            send_interface(arg, 0x23, CANTxData, 5);
        }
            break;

            // 0x10~0x1F CMDs with Memory
        case 0x11:  // Set Node-ID and Store to EEPROM
            boardConfig.canNodeId = *(uint32_t*) (RxData);
            if (RxData[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x12:  // Set Current-Limit and Store to EEPROM
            motor.config.motionParams.ratedCurrent = (int32_t) (*(float*) RxData * 1000);
            boardConfig.currentLimit = motor.config.motionParams.ratedCurrent;
            if (RxData[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x13:  // Set Velocity-Limit and Store to EEPROM
            motor.config.motionParams.ratedVelocity =
                (int32_t) (*(float*) RxData *
                           (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
            boardConfig.velocityLimit = motor.config.motionParams.ratedVelocity;
            if (RxData[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x14:  // Set Acceleration （and Store to EEPROM）
            tmpF = *(float*) RxData * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;

            motor.config.motionParams.ratedVelocityAcc = (int32_t) tmpF;
            motor.motionPlanner.velocityTracker.SetVelocityAcc((int32_t) tmpF);
            motor.motionPlanner.positionTracker.SetVelocityAcc((int32_t) tmpF);
            boardConfig.velocityAcc = motor.config.motionParams.ratedVelocityAcc;
            if (RxData[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x15:  // Apply Home-Position and Store to EEPROM
            motor.controller->ApplyPosAsHomeOffset();
            boardConfig.encoderHomeOffset = motor.config.motionParams.encoderHomeOffset %
                                            motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
            boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x16:  // Set Auto-Enable and Store to EEPROM
            boardConfig.enableMotorOnBoot = (*(uint32_t*) (RxData) == 1);
            if (RxData[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x17:  // Set DCE Kp
            motor.config.ctrlParams.dce.kp = *(int32_t*) (RxData);
            boardConfig.dce_kp = motor.config.ctrlParams.dce.kp;
            if (RxData[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x18:  // Set DCE Kv
            motor.config.ctrlParams.dce.kv = *(int32_t*) (RxData);
            boardConfig.dce_kv = motor.config.ctrlParams.dce.kv;
            if (RxData[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x19:  // Set DCE Ki
            motor.config.ctrlParams.dce.ki = *(int32_t*) (RxData);
            boardConfig.dce_ki = motor.config.ctrlParams.dce.ki;
            if (RxData[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x1A:  // Set DCE Kd
            motor.config.ctrlParams.dce.kd = *(int32_t*) (RxData);
            boardConfig.dce_kd = motor.config.ctrlParams.dce.kd;
            if (RxData[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x1B:  // Set Enable Stall-Protect
            motor.config.ctrlParams.stallProtectSwitch = (*(uint32_t*) (RxData) == 1);
            boardConfig.enableStallProtect = motor.config.ctrlParams.stallProtectSwitch;
            if (RxData[4])
                boardConfig.configStatus = CONFIG_COMMIT;
            break;


            // 0x20~0x2F Inquiry CMDs
        case 0x21: // Get Current
        {
            tmpF = motor.controller->GetFocCurrent();
            memcpy(CANTxData, &tmpF, 4);
            CANTxData[4] = (motor.controller->state == Motor::STATE_FINISH ? 1 : 0);
            send_interface(arg, 0x21, CANTxData, 5);
        }
            break;
        case 0x22: // Get Velocity
        {
            tmpF = motor.controller->GetVelocity();
            memcpy(CANTxData, &tmpF, 4);
            CANTxData[4] = (motor.controller->state == Motor::STATE_FINISH ? 1 : 0);
            send_interface(arg, 0x22, CANTxData, 5);
        }
            break;
        case 0x23: // Get Position
        {
            tmpF = motor.controller->GetPosition();
            memcpy(CANTxData, &tmpF, 4);
            // Finished ACK
            CANTxData[4] = motor.controller->state == Motor::STATE_FINISH ? 1 : 0;
            send_interface(arg, 0x23, CANTxData, 5);
//            debugPrintf("CAN SEND BACK to NODE[%d]\n", boardConfig.canNodeId );
        }
            break;
        case 0x24: // Get Offset
        {
            tmpI = motor.config.motionParams.encoderHomeOffset;
            memcpy(CANTxData, &tmpI, 4);
            send_interface(arg, 0x24, CANTxData, 4);
        }
            break;

        case 0x25: // Get temperature
        {
            tmpF = boardConfig.motor_temperature;
            memcpy(CANTxData, &tmpF, 4);
            send_interface(arg, 0x25, CANTxData, 4);
        }
            break;
        case 0x7d:  // enable motor temperature watch
			boardConfig.enableTempWatch = true;
			if (RxData[4])
				boardConfig.configStatus = CONFIG_COMMIT;
            break;
        case 0x7e:  // Erase Configs
            boardConfig.configStatus = CONFIG_RESTORE;
            break;
        case 0x7f:  // Reboot
            HAL_NVIC_SystemReset();
            break;
        default:
            break;
    }
}


